#include "dynamicSolver.h"
#include <iostream>
#include <sstream>
#include "color.h"
//#include <omp.h>

#define LIMITE 0.0000001

DynamicSolver::DynamicSolver(std::vector<Fabric *> *_fabrics, std::vector<Object *> *_objects,
                             unsigned int _method, double _deltaT, double _gravity, bool _activeCollision,
                             double _collisionDampingFactor, double _collisionDistanceError,
                             bool _activeAir, int _airType, double _airPeriod, double _airRepeat, double _airViscosity, Vector3D _airDirection)
                                : Solver(_fabrics, _objects, _method, _deltaT, _gravity, _activeCollision,
                                         _collisionDampingFactor, _collisionDistanceError,
                                         _activeAir, _airType, _airPeriod, _airRepeat, _airViscosity, _airDirection){
}

DynamicSolver::~DynamicSolver()
{

}

//updating the positions and velocities
void DynamicSolver::calculate(bool _record)
{
//    std::cout <<"\tcomeço do dinamic solver" <<std::endl;


    totalFrames++;
    QTime frameTimer;
    frameTimer.start();


    QString str = "";

    Fabric *fabric;

//    #pragma omp parallel for schedule(dynamic)
    for(unsigned int j = 0; j < fabrics->size(); j++)
    {
        bool collided8 = false;
        Vector3D velocity8;
        Vector3D position8;



        fabric = fabrics->at(j);

        //para caso haja alguma modificação externa
        method->setCurrentState(j, fabric->getState());

        QTime methodTimer;
        if(firstIteration)
        {
            methodTimer.start();
        }

        method->calculate(firstIteration, j, 1);//o ultimo é a fração usada do DeltaT, será ajeitado de acordo com que formos fazendo a colisão

        if(firstIteration)
        {
            totalMethodTime += methodTimer.elapsed();

            std::cout << "---- O método levou " << totalMethodTime <<" milisegundos na primeira iteração" <<std::endl;
        }

        const Matrix &ref = fabric->getState().getReferenceMatrix();
        const Vector &position = method->getPosition(j);
        const Vector &velocity = method->getVelocity(j);


        if(_record){
            str.append( QString("t ") );
            str.append( QString::number(j) );
            str.append( QString(" ") );
            str.append( QString::number(fabric->getState().getT() + method->getDeltaT()) );
        }

        unsigned int size = fabric->getParticlesSize();

        //#pragma omp parallel for //schedule(dynamic)
        for(unsigned int i = 0; i < size; i++)
        {
            double px, py, pz, vx, vy, vz;
            if (ref[i][0] != -1)
            {
                vx = velocity[ref[i][0]];
                px = position[ref[i][0]];
            }
            else
            {
                vx = fabric->getParticle(i)->getVelocity().x;
                px = fabric->getParticle(i)->getPosition().x;
            }
            if (ref[i][1] != -1)
            {
                vy = velocity[ref[i][1]];
                py = position[ref[i][1]];
            }
            else
            {
                vy = fabric->getParticle(i)->getVelocity().y;
                py = fabric->getParticle(i)->getPosition().y;
            }
            if (ref[i][2] != -1)
            {
                vz = velocity[ref[i][2]];
                pz = position[ref[i][2]];
            }
            else
            {
                vz = fabric->getParticle(i)->getVelocity().z;
                pz = fabric->getParticle(i)->getPosition().z;
            }


            Vector3D newPosition(px, py, pz);
            Vector3D newVelocity(vx, vy, vz);


            //Testando colisão
            //checa se há interseção com algum objeto
            //pega o objeto que teve a primeira interseção para ser tratado

            if(testCollision)
            {
                Object *collidedObject = NULL;
                double minDistance = collisionDistanceError;//qualquer valor acima do erro é valido
                for(unsigned int l = 0; l < objects->size(); l++)
                {
                    double distance = objects->at(l)->intersection(newPosition);
                    if(distance < minDistance)
                    {
                        minDistance = distance;
                        collidedObject = objects->at(l);
                    }
                }


                if(collidedObject)
                {

                    //pegnado a normal no ponto de interseção
                    Vector3D normal = collidedObject->getNormalAt(newPosition);

                    //calculando a componente da velocidade na direção da normal
                    Vector3D normalComponent = normal*(newVelocity.dotProduct(normal));

                    //calculando a nova velocidade e posição da particula infratora
                    newVelocity = newVelocity - normalComponent - normalComponent*collisionDampingFactor;

                    newPosition = newPosition + newVelocity*method->getDeltaT();


                    if(i == 8)
                    {
                        std::cout <<"vai prender a 8" <<std::endl;
                        collided8 = true;
                        position8 = newPosition;
                        velocity8 = newVelocity;
                    }
                }

            }

            fabric->getParticle(i)->setVelocity(newVelocity);
            fabric->getParticle(i)->setPosition(newPosition);

            method->setCurrentStateInParticle(j, i, newPosition, newVelocity);


            if(_record)
            {
                str.append( QString(" ") );
                str.append( QString::number(px) );
                str.append( QString(" ") );
                str.append( QString::number(py) );
                str.append( QString(" ") );
                str.append( QString::number(pz) );
            }
        }
        if(_record) str.append( tr("\n") );

        if(collided8) method->setCurrentStateInParticle(j, 8, true, true, true, position8, velocity8);
        //recalcular os states
        fabric->setState(method->getCurrentState(j));
       //fabric->setState(temp);

        fabric->updateSpringsLength();
    }

    if(_record)
    {
        if(--recordFrameIn == 0)
        {
            emit recordSimulation(str);
            recordFrameIn = recordFrameInTotal;
        }
    }
    firstIteration = false;

//    QTime frameEnd = QTime::currentTime();

//    totalFramesTime = sumTimes(totalFramesTime, subtractTimes(frameEnd,frameBegin));
    totalFramesTime += frameTimer.elapsed();

//    double time_ = fabrics->at(0)->getState().getT();
//    double stopT1 = 0.3;
//    double stopT2 = 0.6;
//    double stopT3 = 1.2;
//    double stopT4 = 3.0;

//    if(
//          (time_ >= stopT1 - LIMITE && time_ <= stopT1 + method->getDeltaT() + LIMITE) ||
//        (time_ >= stopT2 - LIMITE && time_ <= stopT2 + method->getDeltaT() + LIMITE) ||
//        (time_ >= stopT3 - LIMITE && time_ <= stopT3 + method->getDeltaT() + LIMITE) ||
//        (time_ >= stopT4 - LIMITE && time_ <= stopT4 + method->getDeltaT() + LIMITE) )
//    {
//        std::cout <<" TEMPO: "<<time_<< "\n\tLevou até agora " << totalFramesTime <<" milisegundos" <<
//                                        "\n\tPrecisou de " << totalFrames << "frames" <<
//                                        "\n\tFPS" << (double)totalFrames/(double)totalFramesTime <<std::endl <<std::endl;
//        emit pause(false);
//    }
//    std::cout <<"\tfim do dinamic solver" <<std::endl;
}
